1. Field of the Invention
The present invention relates to a robot controller for a robot including a plurality of joints.
2. Description of the Related Art
Typically, a robot controller for a robot which performs a required motion by moving a plurality of joints sequentially determines a manipulation amount (control input) for controlling a motion state of each joint (such as a displacement amount of each joint or a drive force of each joint), and controls the motion state of each joint via an actuator such as an electric motor according to the manipulation amount.
In such robot control, it is often necessary to calculate a pseudo inverse matrix of a matrix (generally, a matrix that is not invertible), such as a Jacobian matrix, representing a linear map, and perform a calculation process that uses the pseudo inverse matrix, in order to determine the manipulation amount.
For example, Japanese Patent Application Laid-Open No. 2006-150567 (hereafter referred to as Patent Document 1) describes a technique of determining a desired angular velocity of each joint of a robot from a ZMP velocity by using a pseudo inverse matrix of a ZMP Jacobian matrix that associates an angular change of each joint of the robot with a ZMP change.
Let A be a matrix whose pseudo inverse matrix is to be calculated. The pseudo inverse matrix (hereafter denoted by A*) of the matrix A is typically calculated according to the following equation (1). Note that the superscript “T” denotes transposition.A*=AT·(A·AT)−1  (1)
However, in the case where a magnitude of a determinant det(A·AT) of a matrix (A·AT) in the right side of the equation (1) is extremely small such as zero or close to zero, the calculation result of the right side of the equation (1) diverges, as a result of which the correct pseudo inverse matrix A* cannot be calculated.
To prevent this, there is a known method of calculating the pseudo inverse matrix A* according to the following equation (2) as proposed in, for example, Nakamura Y. and Hanafusa H., “Inverse Kinematic Solutions with Singularity Robustness for Robot Manipulator Control”, Journal of Dynamic Systems, Measurement, and Control, Vol. 108, September (1986), pp. 163-171 (hereafter referred to as Non-patent Document 1).A*=AT·(A·AT+k·I)−1  (2)
In the equation (2), k is an adjustment parameter that is set to an appropriate real number equal to or more than zero in order to prevent a magnitude of a determinant det(A·AT+k·I) (hereafter denoted by DET) of a matrix (=A·AT+k·I) inside the parentheses in the right side of the equation (2) from becoming excessively small. I is a unit matrix.
In the case of calculating the pseudo inverse matrix A* according to the equation (2), the magnitude of the determinant DET can be prevented from becoming excessively small, by appropriately setting the value of the adjustment parameter k. As a result, the pseudo inverse matrix A* can be kept from diverging.